use num_traits::Pow;
extern crate nalgebra as na;
use crate::tools::tool::{self, gaussian_likehood};
use na::*;
use nalgebra::{
    base::allocator::Allocator, DefaultAllocator, Dim, DimName, DimNameAdd, MatrixMN, RealField,
    Scalar,
};
extern crate docify;
use nalgebra;
use nalgebra::DimMinimum;
use rand::prelude::*;
use statrs::distribution::{Continuous, Normal};
use statrs::statistics::Distribution;
use std::f64::consts::PI;
use std::ops::Add;

/// input u=[v,yaw_rate]
fn motion_model(x_vec: &Vec<f64>, u_vec: &Vec<f64>, dt: f64) -> Matrix4x1<f64> {
    let x = Matrix4x1::<f64>::from_vec(x_vec.to_vec());
    let u = Matrix2x1::<f64>::from_vec(u_vec.to_vec());
    let mut F = Matrix4::<f64>::identity();
    F[(3, 3)] = 0.;
    let v = u[(0, 0)];
    let yaw_rate = u[(1, 0)];
    let yaw = x[(2, 0)];
    let delta_x = dt * yaw.cos();
    let delta_y = dt * yaw.sin();
    let B = Matrix4x2::new(delta_x, 0., delta_y, 0., 0., dt, 1., 0.);
    let res = F * x + B * u;
    res
}
fn cal_covariance(x: &Vec<f64>, px: &Vec<Vec<f64>>, pw: &Vec<f64>) -> Matrix4<f64> {
    let mut cov = Matrix4::<f64>::zeros();
    let np = pw.len();
    for i in 0..np {
        let pxi = &px[i];
        let dif: Vec<f64> = x.iter().zip(pxi).map(|v| v.1 - v.0).collect();
        let dx = Matrix4x1::from_vec(dif);
        let _cov = dx * dx.transpose() * pw[i];
        cov = cov + _cov;
    }
    let dot_pw: f64 = pw.iter().map(|v| v * v).sum();
    cov = cov / (1. - dot_pw);
    cov
}
pub struct Particle {
    ///控制变量方差  也可以是协方差,采样要改
    pub u_sigma: Vec<f64>,
    ///  观测 标准差
    pub observ_sigma: f64,
    /// 粒子集合
    pub px: Vec<Vec<f64>>,
    /// 粒子对应概率
    pub pw: Vec<f64>,
}

#[doc = docify::embed!("src/robotics/particle_filter.rs", test_particle)]
impl Particle {
    pub fn sample_one(&mut self, observe: &Vec<Vec<f64>>, u: &Vec<f64>, index: usize, dt: f64) {
        let u_rnd: Vec<f64> = u
            .iter()
            .zip(self.u_sigma.clone())
            .map(|v| v.0 + tool::random_from_stdnorm() * v.1)
            .collect();
        ///根据运动方程,加了概率误差的u得到新的x
        let x = motion_model(&self.px[index], &u_rnd, dt);
        let n_landmark = observe.len();
        let mut w = self.pw[index];

        for i in 0..n_landmark {
            ///x is [x,y,yaw,v] observe is [[dis,x,y]]
            let dx = x[(0, 0)] - observe[i][1];
            let dy = x[(1, 0)] - observe[i][2];
            /// 本采样点和观测xy计算距离,此外还观测了距离,2距离diff
            let predis = (dx * dx + dy * dy).sqrt();
            /// 均值zero,观测值标准差的正态分布确定diff的概率,
            ///  综合了以前这个采样的w,和本次多个路标的w
            let diff = predis - observe[i][0];
            let norm = Normal::new(0.0, self.observ_sigma).unwrap();
            w = w * norm.pdf(diff);
        }
        let clone_x = x.as_slice().to_vec();
        self.px[index] = clone_x;
        self.pw[index] = w;
    }
    pub fn est_final(&self) -> Vec<f64> {
        let mut est_x = Vec::new();
        let cols = self.px[0].len();
        for col in 0..cols {
            let col_data: Vec<f64> = self.px.iter().map(|v| v[col]).collect();
            let after_p: f64 = col_data
                .iter()
                .zip(self.pw.clone())
                .map(|v| v.0 * v.1)
                .sum();
            est_x.push(after_p);
        }
        est_x
    }
    pub fn resample(&mut self, observe: &Vec<Vec<f64>>, u: &Vec<f64>, dt: f64) -> Vec<f64> {
        let np = self.pw.len();
        for index in 0..np {
            self.sample_one(observe, u, index, dt);
        }
        let total_p: f64 = self.pw.iter().sum();
        self.pw = self.pw.iter().map(|v| v / total_p).collect();
        let x_est = self.est_final();
        cal_covariance(&x_est, &self.px, &self.pw);
        /// 得到了估计后,更新粒子集合
        let resample_index = tool::resample_cumsum(&self.pw, self.pw.len());
        let newpx: Vec<Vec<f64>> = resample_index.iter().map(|i| self.px[*i].clone()).collect();
        self.px = newpx;
        x_est
    }
}

#[test]
#[docify::export]
fn test_particle() {
    /// x===x,y,yaw v   u== v,yaw
    let mut x = vec![0., 0., 0.];
    /// dis to car ; x,y  路标的坐标是知道的,距离是测量出来的
    let landmark0 = vec![9.78363848, 10., 0.];
    let landmark1 = vec![14.48872245, 10., 10.];
    let landmark2 = vec![15.22187153, 0., 15.];
    let landmarks = vec![landmark0, landmark1, landmark2];
    /// velocity ,yaw_rate
    let u = vec![2.22781575, 0.92108407];
    let x0 = Matrix4x1::<f64>::zeros();
    let dt = 0.1;
    // let res = motion_model(x0, u, dt);
    let pw = vec![0.5, 0.5];
    let px = vec![
        vec![2.37331233, 3.27599214, 0.96209509, 2.22781575],
        vec![1.37331233, 2.27599214, 0.96209509, 1.22781575],
    ];
    let mut p = Particle {
        pw: pw,
        px: px,
        observ_sigma: 4.,
        u_sigma: vec![4., 0.48738787],
    };
    p.resample(&landmarks, &u, 0.1);
    // let x_est = vec![1.81802421, 0.7018705, 0.82897566, 2.22781575];
    // cal_covariance(x_est, px, pw);
}
